#include "robo_app.h"

char traceBuffer[100] = {0};
uint32_t itCounter = 0;
/*
 * Possible value for ICM frequency
 */
//#define PROCESSING_FREQUENCY 1000 /*Hz*/
#define PROCESSING_FREQUENCY 200 /*Hz*/
//#define PROCESSING_FREQUENCY 100 /*Hz*/

void spi3csen()
{
	ICMNSS_ENABLE;
}
void spi3csdis()
{
	ICMNSS_DISABLE;
}

/* 
 * Select communication link between STNucleo and ICM426xx 
 */
//#define SERIF_TYPE ICM426XX_UI_SPI3
#define SERIF_TYPE ICM426XX_UI_SPI4
//#define SERIF_TYPE ICM426XX_UI_I2C

/* 
 * Define msg level 
 */
#define MSG_LEVEL INV_MSG_LEVEL_DEBUG


#define TO_MASK(a) (1U << (unsigned)(a))

/* --------------------------------------------------------------------------------------
 *  Static variables
 * -------------------------------------------------------------------------------------- */

/* Flag set from icm426xx device irq handler */
static  int irq_from_device = 0;

/* Flag set once a UART RX frame is received */
volatile int irq_event_main_uart = 0;
uint8_t uart1RecBuffer[16] ;

static void SetupMCUHardware(struct inv_icm426xx_serif * icm_serif);
static void ext_interrupt_inv_cb(void * context, int int_num);
void check_rc(int rc, const char * msg_context);
void msg_printer1(int level, const char * str, va_list ap);

void imuReadTest(void);
void TestLoop(void);

//RCC_ClocksTypeDef get_rcc_clock;
void roboInit(void)
{
//	TestLoop();
	HAL_Delay(1000);
	HAL_Delay(1000);

	
	int rc = 0;
	struct inv_icm426xx_serif icm426xx_serif;
	LL_SPI_SetRxFIFOThreshold(SPI1,LL_SPI_RX_FIFO_TH_QUARTER);
	LL_SPI_Enable(ICMSPI);

//	RCC_GetClocksFreq(&get_rcc_clock);
	INV_MSG(INV_MSG_LEVEL_INFO,"SystemCoreClock : %u\r\n",SystemCoreClock );
	
	/* Initialize MCU hardware */
	SetupMCUHardware(&icm426xx_serif);
	/* Initialize Icm426xx */
	rc = SetupInvDevice(&icm426xx_serif);
	check_rc(rc, "error while setting up INV device");
	
	rc = InitInvRobovacAlgo(PROCESSING_FREQUENCY);
	check_rc(rc, "error while initializing Robovac algorithm");
	
	/* Configure Icm426xx */
	rc = ConfigureInvDevice(PROCESSING_FREQUENCY);
	check_rc(rc, "error while configuring INV device");
	
	INV_MSG(INV_MSG_LEVEL_INFO, "Start processing");
	
//	imuReadTest();


//	while(1);
	
}

extern int istate;
extern InvnAlgoRobovacInput input;
extern InvnAlgoRobovacOutput output;
void process_user_command(char c, int* state);

uint64_t usTickGap = 0;
uint64_t ustick1 = 0, ustick2 = 0;

#define ROBO_STOP	0
#define ROBO_RUN	1
void roboLoop(void)
{
	int rc = 0;
	static uint32_t lastTick = 0, nowTick = 0;
	static uint32_t delayStart = 0, delayOn = 0;
	static uint32_t nowState = ROBO_STOP;


	/* Check Icm426xx IRQ */
	if ( irq_from_device ) 
	{

		rc = GetDataFromInvDevice();

		usTickGap = ustick2-ustick1;
		check_rc(rc, "error while getting data from Icm426xx");

		__disable_irq();
		irq_from_device = 0;
		__enable_irq();

		updateYawStopRequest();
		
//		lastTick = nowTick;
//		nowTick = HAL_GetTick();
//		if ((nowTick/20)!=(lastTick/20))
//		{

//			updateYawStopRequest();
//		}
		
		if (output.stop_request && nowState == ROBO_RUN)
		{
			process_user_command('s', &istate);
			nowState = ROBO_STOP;
		}
		else if((!output.stop_request) && nowState == ROBO_STOP)
		{
			process_user_command('c', &istate);
			nowState = ROBO_RUN;
		}
	
		
	#if (0)
		if (output.stop_request)
		{
//			if (input.sRobotState == MOTOR_ON)
//			{
				if (delayOn == 0)
				{
					//sendStopCMD to robot
//					HAL_UART_Transmit(&huart2,"@008\r\n",6,100);
					printf("robot stop \r\n");
					delayOn = 1;
					delayStart = HAL_GetTick();
				}



				if ( delayOn == 1 && (nowTick-delayStart) > 500)
				{
					process_user_command('s', &istate);
					delayOn = 0;
				}


//			}

		}
		else
		{
			if (input.sRobotState != CLEAN)
			{
				printf("robot go \r\n");
				process_user_command('c', &istate);
				//sendCleanCMD to robot
//				HAL_UART_Transmit(&huart2,"@0074\r\n",7,100);
			}
		}
	#endif
		
	}

	
}
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
//	printf("%EXIT now u\r\n",GPIO_Pin);
//	printf ("it");
	ext_interrupt_inv_cb(NULL,9);
}
/*
 * This function initializes MCU on which this software is running.
 * It configures:
 *   - a UART link used to send traces to a terminal
 *   - interrupt priority group and GPIOs so that MCU can receive interrupts from both ICM426xx
 *   - a microsecond timer requested by Icm426xx driver to compute some delay
 *   - a microsecond timer used to get some timestamps
 *   - a serial link to communicate from MCU to Icm426xx
 *   - the LED and the push-button (PB) to interact simulate the iRobot TODO
 */
static void SetupMCUHardware(struct inv_icm426xx_serif * icm_serif)
{


	/* Setup message facility to see internal traces from FW */
//	printf("why don't exe here?\n");
	
	HAL_UART_Receive_IT(&huart1,uart1RecBuffer,1);
	
	INV_MSG_SETUP(MSG_LEVEL, msg_printer1);

	INV_MSG(INV_MSG_LEVEL_INFO, "###################################");
	INV_MSG(INV_MSG_LEVEL_INFO, "#  ICM426xx streamlined example   #");
	INV_MSG(INV_MSG_LEVEL_INFO, "###################################");
	


	/*
	 * Configure input capture mode GPIO connected to pin PB10 (arduino connector D6).
	 * This pin is connected to Icm426xx INT1 output and thus will receive interrupts 
	 * enabled on INT1 from the device.
	 * A callback function is also passed that will be executed each time an interrupt
	 * fires.
	*/
//	gpio_sensor_irq_init(TO_MASK(GPIO_SENSOR_IRQ_D6), ext_interrupt_inv_cb, 0);
	/* EXTI interrupt init*/
	
	HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0);
	HAL_NVIC_EnableIRQ(EXTI0_IRQn);


//	/* Init timer peripheral for delay */
//	delay_init(DELAY_TIMER);
	DWT_Init();	//����DWT��ʱ��, ΢����ʱ��
	ICMNSS_DISABLE;

//	rtc_timer_init(NULL, 0, RTC_TIMER_CLOCK_LSE);
//	gpio_output_clk_on_pin(); /* Output 32kHz LSE to PA8 */

	/* Initialize serial inteface between MCU and Icm426xx */
	icm_serif->context   = 0;        /* no need */
	icm_serif->read_reg  = inv_io_hal_read_reg;
	icm_serif->write_reg = inv_io_hal_write_reg;
	icm_serif->max_read  = 1024*32;  /* maximum number of bytes allowed per serial read */
	icm_serif->max_write = 1024*32;  /* maximum number of bytes allowed per serial write */
	icm_serif->serif_type = SERIF_TYPE;
//	inv_io_hal_init(icm_serif);	
}

void ext_interrupt_inv_cb(void * context, int int_num)
{
	(void)context;

//	irq_from_device |= TO_MASK(int_num);
	irq_from_device = 1;
	itCounter++;
}

/*
 * Printer function for message facility
 */
void msg_printer1(int level, const char * str, va_list ap)
{
	return;
	static char out_str[256]; /* static to limit stack usage */
	unsigned idx = 0;
	const char * s[INV_MSG_LEVEL_MAX] = {
		"",    // INV_MSG_LEVEL_OFF
		"[E] ", // INV_MSG_LEVEL_ERROR
		"[W] ", // INV_MSG_LEVEL_WARNING
		"[I] ", // INV_MSG_LEVEL_INFO
		"[V] ", // INV_MSG_LEVEL_VERBOSE
		"[D] ", // INV_MSG_LEVEL_DEBUG
	};
	if(level == INV_MSG_LEVEL_DEBUG) return;
	idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "%s", s[level]);
	if(idx >= (sizeof(out_str)))
		return;
	idx += vsnprintf(&out_str[idx], sizeof(out_str) - idx, str, ap);
	if(idx >= (sizeof(out_str)))
		return;
	idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "\r\n");
	if(idx >= (sizeof(out_str)))
		return;

//	CDC_Transmit_FS(out_str,idx);
//	HAL_UART_Transmit(&huart1,out_str,idx,10);
	HAL_UART_Transmit_DMA(&huart1,out_str,idx);
}



uint64_t inv_icm426xx_get_time_us(void)
{
	uint32_t nowTick = SysTick->VAL;

	return (uint64_t)nowTick/((SysTick->LOAD+1)/1000) + HAL_GetTick()*1000;
	
}
void inv_icm426xx_sleep_us(uint32_t us)
{
	DWT_delay_us(us);
}
void inv_helper_enable_irq(void)
{
	enable_irq();
}
void inv_helper_disable_irq(void)
{
	disable_irq();
}
	
void check_rc(int rc, const char * msg_context)
{
	if(rc < 0) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "%s: error %d (%s)\r\n", msg_context, rc, inv_error_str(rc));
		while(1);
	}
}
	
int fputc(int c, FILE *stream)
{
	USART1->TDR = c;
	while(!(USART1->ISR & UART_FLAG_TXE)){};
	return c;
}

uint8_t printfBuffer[512];
void usb_printf(const char *format, ...)
{
	uint32_t length;
	va_list args;
 
	va_start(args, format);
	length = vsnprintf((char*)printfBuffer, sizeof(printfBuffer), (char*)format, args);
	va_end(args);
//	CDC_Transmit_FS((const char*)printfBuffer,length);
	HAL_UART_Transmit_DMA(&huart1,printfBuffer,length);
	
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	if (huart == &huart1)
	{
		irq_event_main_uart = 1;
		INV_MSG(INV_MSG_LEVEL_INFO, "uart1 recv:  %c\r\n",uart1RecBuffer[0]);
		HAL_UART_Receive_IT(&huart1,uart1RecBuffer,1);
	}
}

uint8_t uart_getc(void)
{
	return uart1RecBuffer[0];
}

#define WAIT_FOR_FLAG(flag, timeout, errorcode) Timeout = timeout;\
            while(!flag)  {\
              if((Timeout--) == 0) return errorcode; \
            }\

uint8_t spillTR1byte(uint8_t dat)
{
	uint8_t datout = 88;
	int Timeout = 0;
		WAIT_FOR_FLAG (LL_SPI_IsActiveFlag_TXE (SPI1),100,6);
//	while((SPI3->SR & 1<<1) == 0);
		LL_SPI_TransmitData8 (SPI1, dat); //Dummy byte ro genarate clock
	
//	while((SPI3->SR & 1<<0) == 0);
		WAIT_FOR_FLAG (!LL_SPI_IsActiveFlag_BSY (SPI1),100,5);
		datout = LL_SPI_ReceiveData8 (SPI1);

	return datout;
}

uint8_t g_outp[10];
uint8_t g_in = 0;
void SpiTest(void)
{
	ICMNSS_ENABLE;
	g_in = 0x75|0x80;
	g_outp[0] = 0;
//	HAL_SPI_TransmitReceive(&hspi1,&g_in,g_ret,1,10);
//	HAL_SPI_TransmitReceive(&hspi1,&g_in,g_ret,1,10);
	spillTR1byte(g_in);
	g_outp[0] = spillTR1byte(g_in);
	ICMNSS_DISABLE;
	TR_TRACE("0x%X",g_outp[0]);
	HAL_Delay(200);
}
void imuReadTest(void)
{
	uint8_t in = 0, out = 0;
	int ret = 0;

	while (1)
	{
		inv_io_hal_read_reg(NULL, 0x75, g_outp, 1);
		TR_TRACE("0x%X",g_outp[0]);
		HAL_Delay(200);
//		SpiTest();
	}
}

uint64_t usTick1 = 0, usTick2 = 0, usTick3 = 0; 
void TestLoop(void)
{
	while (1)
	{
		usTick1 = inv_icm426xx_get_time_us();
		HAL_Delay(1);
		usTick2 = inv_icm426xx_get_time_us();
		usTick3 = usTick2 - usTick1;
	}
}
